/**
  ******************************************************************************
  * @file    motor.c
  * @brief   编码电机驱动实现，电机驱动为TB6612
  * @version V1.0.0
  * @date    2025-07-15
  ******************************************************************************
  */
	
#include "motor.h"

/**
  * @brief 电机12初始化
  * @retval None
  */
  
void motor12_init(void)
{
	HAL_TIM_PWM_Init(&motor_TIM);
	HAL_TIM_PWM_Start(&motor_TIM,motor1_TIM_CHANNEL);
	HAL_TIM_PWM_Start(&motor_TIM,motor2_TIM_CHANNEL);
} 

/**
  * @brief 电机34初始化
  * @retval None
  */
  
void motor34_init(void)
{
	HAL_TIM_PWM_Init(&motor_TIM);
	HAL_TIM_PWM_Start(&motor_TIM,motor3_TIM_CHANNEL);
	HAL_TIM_PWM_Start(&motor_TIM,motor4_TIM_CHANNEL);
} 


/**
	* @brief  控制电机速度
	* @param  speed1 Moter1的速度
	* @param  speed2 Moter2的速度
	* @param  speed3 Moter3的速度
	* @param  speed4 Moter4的速度
  * @retval None
	* @note   PWM频率为2000Hz
  */
void Moter_SetSpeed(short speed1, short speed2, short speed3, short speed4)
{
	if(speed1 > 0)
	{
		MOTER1_PIN_1(1);
		MOTER1_PIN_2(0);
	}
	else if(speed1 < 0)
	{
		MOTER1_PIN_1(0);
		MOTER1_PIN_2(1);
	}
	if(speed2 > 0)
	{
		MOTER2_PIN_1(1);
		MOTER2_PIN_2(0);
	}
	else if(speed2 < 0)
	{
		MOTER2_PIN_1(0);
		MOTER2_PIN_2(1);
	}
	if(speed3 > 0)
	{
		MOTER3_PIN_1(1);
		MOTER3_PIN_2(0);
	}
	else if(speed3 < 0)
	{
		MOTER3_PIN_1(0);
		MOTER3_PIN_2(1);
	}
	if(speed4 > 0)
	{
		MOTER4_PIN_1(1);
		MOTER4_PIN_2(0);
	}
	else if(speed4 < 0)
	{
		MOTER4_PIN_1(0);
		MOTER4_PIN_2(1);
	}
	__HAL_TIM_SetCompare(&motor_TIM, motor1_TIM_CHANNEL, MOTER_ABS(speed1));
	__HAL_TIM_SetCompare(&motor_TIM, motor2_TIM_CHANNEL, MOTER_ABS(speed2));
	__HAL_TIM_SetCompare(&motor_TIM, motor3_TIM_CHANNEL, MOTER_ABS(speed3));
	__HAL_TIM_SetCompare(&motor_TIM, motor4_TIM_CHANNEL, MOTER_ABS(speed4));
}
